#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <opencv2/opencv.hpp>
// 加入Eigen库
#include "ArmorBase.h"
namespace hitcrt {
class ArmorCalculator {
   public:
    ArmorCalculator(const double pitchCameraToRobot,
                    const double yawCameraToRobot, const double width,
                    const double largeArmorWidth,
                    const double smallArmorWidth) {
        m_width = width;
        m_largeArmorWidth = largeArmorWidth;
        m_smallArmorWidth = smallArmorWidth;
        m_rvec = cv::Mat::zeros(3, 1, CV_64F);
        m_tvec = cv::Mat::zeros(3, 1, CV_64F);
        m_cameraMatrix = (cv::Mat_<double>(3, 3) << 1.7073e+03, 0, 619.7688, 0,
                          1.7073e+03, 493.8841, 0, 0, 1);
        m_distCoeffs = (cv::Mat_<double>(1, 5) << -0.0733, 0.3134, 0, 0, 0);
        m_pitchCameraToRobot = pitchCameraToRobot;  // 电控发来的pitch角
        m_yawCameraToRobot = yawCameraToRobot;
        
    };
    void rotationCalculation(std::vector<Armor>& armors, cv::Mat& frame);
    void rotationTransalion(cv::Mat tvec, cv::Mat revc, Armor& armor);
    void clearVectors();
    void getCornerPoint(int index, std::vector<Armor>& armors);
    void resultVersion(int index, std::vector<Armor>& armors, cv::Mat& frame);

    std::vector<cv::Point3f> m_pointCornerWorld;  // 世界坐标系下的四个角点
    std::vector<cv::Point2f> m_pointCornerImg;  // 像素坐标系下的四个
    cv::Point3f m_pointCenterImg;  // 装甲板反求解图像坐标
    cv::Point3f m_pointCenterCar;
    cv::Mat m_rvec = cv::Mat::zeros(3, 1, CV_64F);  // 创建旋转和平移矩阵
    cv::Mat m_tvec = cv::Mat::zeros(3, 1, CV_64F);

    cv::Mat m_cameraMatrix = (cv::Mat_<double>(3, 3) << 1.7073e+03, 0, 619.7688,
                              0, 1.7073e+03, 493.8841, 0, 0, 1);
    // 创建相机内参数矩阵

    cv::Mat m_distCoeffs = (cv::Mat_<double>(1, 5) << -0.0733, 0.3134, 0, 0, 0);
    // 创建相机畸变系数矩阵

    double m_pitchCameraToRobot;  // 电控发来的pitch角
    double m_yawCameraToRobot;    // 电控发来的yaw角
    double m_width;               // 装甲板宽度
    double m_height;              // 装甲板长度
    double m_largeArmorWidth;     // 大装甲板宽度
    double m_smallArmorWidth;     // 小装甲板宽度
    double m_shiftZ;  // 相机坐标系到云台坐标系的z轴平移量
    double m_shiftY;  // 相机坐标系到云台坐标系的y轴平移量

    double m_yawSend;
    double m_pitchSend;
};

}  // namespace hitcrt